﻿using System;
using System.IO;
using System.Collections.Generic;
using UnityEngine;


public class BVHDriver : MonoBehaviour
{
    [Header("Loader settings")]
    [Tooltip("This is the target avatar for which the animation should be loaded. Bone names should be identical to those in the BVH file and unique. All bones should be initialized with zero rotations. This is usually the case for VRM avatars.")]
    public Animator targetAvatar;

    [Serializable]
    public struct BoneMap
    {
        public string name;
        public HumanBodyBones humanoid_bone;
    }

    public string filename = "";

    private BoneMap[] bmapsALL; // the corresponding bones between unity and bvh
    private BoneMap[] bmapsBVH; // the corresponding bones between unity and bvh
    private BoneMap[] bmapsNSM; // the corresponding bones between unity and nsm
    private BoneMap[] bmapsNSM_all;
    private BVHParser bp = null;
    private Animator anim;

    private Dictionary<string, Quaternion> bvhT;
    private Dictionary<string, Vector3> bvhOffset;
    private Dictionary<string, string> bvhHireachy;
    private Dictionary<HumanBodyBones, Quaternion> unityT;
    public Dictionary<string, Quaternion> currRotation;

    private int frameIdx;
    private float scaleRatio = 1.0f;
    private string searchKey = "";
    private float frameRate = 60.0f;

    public Dictionary<string, Transform> getAllBonesTransform()
    {
        Dictionary<string, Transform> all_bones = new Dictionary<string, Transform>();

        foreach (BoneMap bm in bmapsALL)
        {
            Transform currBone = anim.GetBoneTransform(bm.humanoid_bone);
            all_bones.Add(bm.name, currBone);
        }

        return all_bones;
    }

    private void search_init()
    {
        // 之后通过 searchkey 进行动作数据库检索，得到 file
        string file = @"Assets\Resources\Bvh\" + filename + ".bvh";
        string bvhData = File.ReadAllText(file);
        bp = new BVHParser(bvhData);
        frameRate = 1f / bp.frameTime;
        Application.targetFrameRate = (Int16)frameRate;

        // bvhT = bp.getTFrame();              // Get bones pose of the first frame, string is bone's name, "pos" stands for root
        bvhT = bp.getKeyFrame(0);              // Get bones pose of the first frame, string is bone's name, "pos" stands for root
        bvhOffset = bp.getOffset(1.0f);     // Get bones offset from bvh
        bvhHireachy = bp.getHierachy();     // Get maps from child bone to parent bone

        //anim = targetAvatar.GetComponent<Animator>();
        //unityT = new Dictionary<HumanBodyBones, Quaternion>();
        //foreach (BoneMap bm in bmapsALL)
        //{
        //    unityT.Add(bm.humanoid_bone, anim.GetBoneTransform(bm.humanoid_bone).rotation); // Get maps from unity_bone to its rotations
        //}

        float unity_leftleg = (anim.GetBoneTransform(HumanBodyBones.LeftLowerLeg).position - anim.GetBoneTransform(HumanBodyBones.LeftUpperLeg).position).sqrMagnitude +
            (anim.GetBoneTransform(HumanBodyBones.LeftFoot).position - anim.GetBoneTransform(HumanBodyBones.LeftLowerLeg).position).sqrMagnitude;

        // TODO unity 与 NSM 骨骼比例
        //float bvh_leftleg = 0.0f;
        //foreach (BoneMap bm in bmapsBVH)
        //{
        //    if (bm.humanoid_bone == HumanBodyBones.LeftUpperLeg || bm.humanoid_bone == HumanBodyBones.LeftFoot)
        //    {
        //        bvh_leftleg = bvh_leftleg + bvhOffset[bm.bvh_name].sqrMagnitude;
        //    }
        //}
        //scaleRatio = bvh_leftleg / unity_leftleg;

        frameIdx = 4;
    }

    public void searchMotion(string skey)
    {
        if (!searchKey.Equals(skey))
        {
            searchKey = skey;
            filename = skey;
            if (searchKey == "")
                return;

            // search motion lib by key to get filename
            search_init();  // initial BVHParser bp            
        }
    }

    public void updateRotations(Dictionary<string, Quaternion> nsmRots, Vector3 rpos)
    {
        if (filename == "")
        {
            foreach (BoneMap bm in bmapsNSM_all) // Compute global rotations
            {
                Transform currBone = anim.GetBoneTransform(bm.humanoid_bone);
                currBone.rotation = nsmRots[bm.name] * unityT[bm.humanoid_bone];
            }
        }
        else
        {
            Quaternion rrot = new Quaternion();
            foreach (BoneMap bm in bmapsNSM) // Compute global rotations
            {
                Transform currBone = anim.GetBoneTransform(bm.humanoid_bone);
                currBone.rotation = nsmRots[bm.name] * unityT[bm.humanoid_bone];
                if (bm.name == "Hips")
                {
                    rrot = currBone.rotation;
                }
            }

            if (searchKey != "")
            {
                if (frameIdx < bp.frames - 1)
                    frameIdx++;
                else
                    frameIdx = 4;

                Dictionary<string, Quaternion> currFrame = bp.getKeyFrameByRrot(frameIdx, rrot);
                foreach (BoneMap bm in bmapsBVH) // Compute global rotations
                {
                    Transform currBone = anim.GetBoneTransform(bm.humanoid_bone);
                    // currBone.rotation = (currFrame[bm.name] * Quaternion.Inverse(bvhT[bm.name])) * unityT[bm.humanoid_bone]; // unityT must add

                    currBone.rotation = currFrame[bm.name] * unityT[bm.humanoid_bone]; // unityT must add
                                                                                       // currBone.rotation =  currFrame[bm.name]; // unityT must add

                    //currBone.rotation = currBone.rotation * rrot;
                    // currBone.localRotation = (currFrame[bm.name] * Quaternion.Inverse(bvhT[bm.name])) * unityT[bm.humanoid_bone]; // unityT must add               
                }

                //draw bvh skeleton
                Dictionary<string, Vector3> bvhPos = new Dictionary<string, Vector3>();
                foreach (string bname in currFrame.Keys)
                {
                    if (bname == "pos")
                    {
                        bvhPos.Add(bp.root.name, new Vector3(currFrame["pos"].x, currFrame["pos"].y, currFrame["pos"].z));
                    }
                    else
                    {
                        if (bvhHireachy.ContainsKey(bname) && bname != bp.root.name)
                        {
                            Vector3 curpos = bvhPos[bvhHireachy[bname]] + currFrame[bvhHireachy[bname]] * bvhOffset[bname];
                            bvhPos.Add(bname, curpos);
                        }
                    }
                }

                anim.GetBoneTransform(HumanBodyBones.Hips).position = bvhPos[bp.root.name] * scaleRatio;
                foreach (string bname in bvhHireachy.Keys)
                {
                    Color color = new Color(1.0f, 0.0f, 0.0f);
                    Debug.DrawLine(bvhPos[bname], bvhPos[bvhHireachy[bname]], color);
                }
            }
        }
        // set root position
        //Vector3 anim_rpos = rpos * scaleRatio; 正确计算后添加
        Vector3 anim_rpos = new Vector3(rpos.x, rpos.y, rpos.z);
        // anim_rpos.y = anim.GetBoneTransform(HumanBodyBones.Hips).position.y;
        anim.GetBoneTransform(HumanBodyBones.Hips).position = anim_rpos;
    }

    private void Start()
    {
        BonemapReader reader = new BonemapReader();
        string path = Params.bonesMapPath + "bvh/";
        reader.Read(path + "Bonesmap.txt");
        bmapsALL = reader.bonemaps;

        reader.Read(path + "Bonesmap_bvh.txt");
        bmapsBVH = reader.bonemaps;

        reader.Read(path + "Bonesmap_nsm.txt");
        bmapsNSM = reader.bonemaps;

        reader.Read(path + "Bonesmap_nsm_all.txt");
        bmapsNSM_all = reader.bonemaps;

        anim = targetAvatar.GetComponent<Animator>();
        unityT = new Dictionary<HumanBodyBones, Quaternion>();
        foreach (BoneMap bm in bmapsALL)
        {
            unityT.Add(bm.humanoid_bone, anim.GetBoneTransform(bm.humanoid_bone).rotation); // Get maps from unity_bone to its rotations
        }
    }

    private void Update()
    {
    }
}

